﻿namespace RLab.Visualization

open System
open System.Linq
open Microsoft.DirectX.Direct3D
open GeometryDefinitions

type PrismaticJointDriveType = 
    | VelocityDrive
    | ForceDrive
    | NoDrive

type JointType = 
    | Revolute 
    | Prismatic of PrismaticJointDriveType

type ChainJoint = 
    { Name : string
      Anchor : V3
      Axis : V3
      Type : JointType }


//type ChainLink = 
//    { Name : string
//      Model : Mesh  //model mesh
//      PhysicModel : V3 * V3 //physic actor: box dimensions * globalPose
//      Mass : single
//      CenterOfMassOffset : V3 }//model mesh offset


type KinematicChain<'a,'b> = 
    | Chain of 'a * Segment<'a,'b>
and Segment<'a,'b> =     
    | Link of 'b * 'a * Segment<'a,'b>
    | End    

type JointNotFoundException()= 
    inherit System.Exception()
   
[<CompilationRepresentation(CompilationRepresentationFlags.ModuleSuffix)>]
module KinematicChain = 
    begin
    let linkIter action kinChain = 
        let rec segmentIter s = 
            match s with
            | Link(_,b,se) -> 
                action b
                segmentIter se
            | End -> ()
        match kinChain with
        | Chain(b,s) -> 
            action b            
            segmentIter s
    let jointIter action kinChain = 
        let rec segmentIter s = 
            match s with
            | Link(j,_,se) -> 
                action j
                segmentIter se
            | End -> ()
        match kinChain with
        | Chain(_,s) -> segmentIter s      
    let links kinChain = 
        seq{ let rec segmentIter s = 
                 seq{ match s with
                      | Link(_,b,se) -> 
                      yield b
                      yield! segmentIter se
                      | End -> yield! Seq.empty }
             match kinChain with
             | Chain(b,s) -> 
                 yield b
                 yield! ( segmentIter s ) }
    let joints kinChain =         
        let rec segmentIter s = 
            seq {match s with
                 | Link(j,_,se) -> 
                     yield j
                     yield! segmentIter se
                 | End -> yield! Seq.empty }
        match kinChain with
        | Chain(_,s) -> (segmentIter s)
    let lastLink kinChain = 
        let rec getLastLink s =
            match s with
            | Link(a,b,End) -> b
            | Link(a,b,c) -> getLastLink c
            | End -> failwith "Logic error!"
        match kinChain with
        | Chain(a,End) -> a
        | Chain(a,s) -> getLastLink s
    let firstJoint (predicate : 'j->bool) kinChain = 
        try
            Enumerable.First((kinChain |> joints),  new System.Func<'j,bool>( predicate ) )
        with :? InvalidOperationException -> raise (JointNotFoundException())
    let firstLink predicate kinChain = Enumerable.First((kinChain |> links), predicate)
    end


